#include "canbus/message.h"

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {

/**
 * @class CanMsg16
 * @brief Concrete can message that stores part of the wheel angle information
 */
class CanMsg16: public CanMsg {
public:
  CanMsg16():
  CanMsg(),
  wheel_angle_ffl(0),
  wheel_angle_ffr(0),
  wheel_angle_fl(0),
  wheel_angle_fr(0)
  {}

  CanMsg16(uint8_t* value): 
  CanMsg(value),
  wheel_angle_ffl(0),
  wheel_angle_ffr(0),
  wheel_angle_fl(0),
  wheel_angle_fr(0) 
  {}

  virtual ~CanMsg16() = default;

  void UpUpdateData() {
    wheel_angle_ffl = -1*Deg2Rad*(ParseTwins(*(data_+1), *(data_+0)) - offset)/unit;
    wheel_angle_ffr = -1*Deg2Rad*(ParseTwins(*(data_+3), *(data_+2)) - offset)/unit; 
    wheel_angle_fl =  -1*Deg2Rad*(ParseTwins(*(data_+5), *(data_+4)) - offset)/unit;
    wheel_angle_fr =  -1*Deg2Rad*(ParseTwins(*(data_+7), *(data_+6)) - offset)/unit;
  }

  template <typename T>
  void DownUpdateData(const T& input_data) {}

  static const int32_t ID;
  static const std::string NAME;
  
  double wheel_angle_ffl;
  double wheel_angle_ffr;
  double wheel_angle_fl; 
  double wheel_angle_fr; 

  const int offset = 5000;
  const int unit = 100;

  friend class CanbusHandler;
};

}